//
// Created by vivi on 07/02/2018.
//

/*
 * Some helper functions for better dealing with Eigen
 */

#pragma once

#include <string>
#include <Eigen/Eigen>
#include <sstream>
#include "VPly/vply.h"

namespace HairEngine {
	namespace EigenUtility {
		/*
		 * Convert a Vector3f into string
		 */
		inline std::string toString(const Eigen::Vector3f & v) {
			std::ostringstream s;
			s << std::showpoint << '{' << v(0) << ',' <<  v(1) << ',' << v(2) << '}';
			return s.str();
		}

		inline std::string toString(const Eigen::Matrix4f & m) {
			std::ostringstream s;
			s << std::showpoint << '{'
			  << '{' << m(0, 0) << ',' << m(0, 1) << ',' << m(0, 2) << ',' <<  m(0, 3) << '}' << ','
			  << '{' << m(1, 0) << ',' << m(1, 1) << ',' << m(1, 2) << ',' << m(1, 3) << '}' << ','
			  << '{' << m(2, 0) << ',' << m(2, 1) << ',' << m(2, 2) << ',' << m(2, 3) << '}' << ','
			  << '{' << m(3, 0) << ',' << m(3, 1) << ',' << m(3, 2) << ',' << m(3, 3) << '}'
			  << '}';
			return s.str();
		}

		inline std::string toString(const Eigen::Matrix3f & m) {
			std::ostringstream s;
			s << std::showpoint << '{'
			  << '{' << m(0, 0) << ',' << m(0, 1) << ',' << m(0, 2) << ','  << '}' << ','
			  << '{' << m(1, 0) << ',' << m(1, 1) << ',' << m(1, 2) << ','  << '}' << ','
			  << '{' << m(2, 0) << ',' << m(2, 1) << ',' << m(2, 2) << ','  << '}' << ','
			  << '}';
			return s.str();
		}

		inline std::ostream &operator<<(std::ostream & os, const Eigen::Vector3f & v) {
			os << '{' << v.x() << ',' << v.y() << ',' << v.z() << '}';
			return os;
		}

		inline std::ostream &operator<<(std::ostream & os, const Eigen::Matrix4f & m) {
			os << '{'
			  << '{' << m(0, 0) << ',' << m(0, 1) << ',' << m(0, 2) << ',' <<  m(0, 3) << '}' << ','
			  << '{' << m(1, 0) << ',' << m(1, 1) << ',' << m(1, 2) << ',' << m(1, 3) << '}' << ','
			  << '{' << m(2, 0) << ',' << m(2, 1) << ',' << m(2, 2) << ',' << m(2, 3) << '}' << ','
			  << '{' << m(3, 0) << ',' << m(3, 1) << ',' << m(3, 2) << ',' << m(3, 3) << '}'
			  << '}';
			return os;
		}

		inline std::ostream &operator<<(std::ostream & os, const Eigen::Matrix3f & m) {
			os << std::showpoint << '{'
			  << '{' << m(0, 0) << ',' << m(0, 1) << ',' << m(0, 2) << ','  << '}' << ','
			  << '{' << m(1, 0) << ',' << m(1, 1) << ',' << m(1, 2) << ','  << '}' << ','
			  << '{' << m(2, 0) << ',' << m(2, 1) << ',' << m(2, 2) << ','  << '}' << ','
			  << '}';
			return os;

		}

		/*
		 * Convert a group of Vector3f into string
		 */
		template <typename Vector3fArrayIterator>
		inline std::string toString(const Vector3fArrayIterator &arrayBegin, const Vector3fArrayIterator & arrayEnd) {
			std::ostringstream s;

			s.setf(std::ios::showpoint);

			for (Vector3fArrayIterator it = arrayBegin; it != arrayEnd; ++it) {
				const Eigen::Vector3f & v = *it;
				if (it != arrayBegin)
					s << ", ";
				s << '{' << v(0) << ',' <<  v(1) << ',' << v(2) << '}';
			}

			return s.str();
		}

		inline VPly::VPlyVector3f toVPlyVector3f(const Eigen::Vector3f & v) {
			return { v(0), v(1), v(2) };
		}

		inline VPly::VPlyMatrix4f toVPlyMatrix4f(const Eigen::Matrix4f & m) {
			return {
					m(0, 0), m(0, 1), m(0, 2), m(0, 3),
					m(1, 0), m(1, 1), m(1, 2), m(1, 3),
					m(2, 0), m(2, 1), m(2, 2), m(2, 3),
					m(3, 0), m(3, 1), m(3, 2), m(3, 3)
			};
		}

		inline VPly::VPlyVector3i toVPlyVector3i(const Eigen::Vector3i & v) {
			return { v.x(), v.y(), v.z() };
		}



		/*
		 * Get a affine3f without translation part
		 */
		inline Eigen::Affine3f getAffine3fWithoutTranslationPart(const Eigen::Affine3f & affine) {
			Eigen::Matrix4f affineInMatrix = affine.matrix();
			affineInMatrix(0, 3) = 0.0f;
			affineInMatrix(1, 3) = 0.0f;
			affineInMatrix(2, 3) = 0.0f;
			return Eigen::Affine3f(std::move(affineInMatrix));
		}

		/*
		 * Get the midpoint of two vector
		 */
		inline Eigen::Vector3f midPoint(const Eigen::Vector3f & p1, const Eigen::Vector3f & p2) {
			return 0.5f * (p1 + p2);
		}

		/*
		 * Get the triangle center of the triangle define by three points
		 */
		inline Eigen::Vector3f triangleCenter(const Eigen::Vector3f & p1, const Eigen::Vector3f & p2, const Eigen::Vector3f & p3) {
			return 1.0f / 3.0f * (p1 + p2 + p3);
		}

		inline void lerpDecompose(const Eigen::Affine3f &aff, Eigen::Vector3f &pos, Eigen::Quaternionf &rot, Eigen::Vector3f &scale)
		{
			Eigen::Matrix3f rot_mat, scale_mat;
			aff.computeRotationScaling(&rot_mat, &scale_mat);

			pos = aff.translation();
			rot = Eigen::Quaternionf(rot_mat);
			scale = scale_mat.diagonal();
		}

		inline Eigen::Affine3f lerpCompose(float alpha,
		                             const Eigen::Vector3f &pos0, const Eigen::Quaternionf &rot0, const Eigen::Vector3f &scale0,
		                             const Eigen::Vector3f &pos1, const Eigen::Quaternionf &rot1, const Eigen::Vector3f &scale1)
		{
			float one_minus_alpha = 1-alpha;

			Eigen::Affine3f result;
			result.fromPositionOrientationScale(
					one_minus_alpha * pos0 + alpha * pos1,
					rot0.slerp(alpha, rot1),
					one_minus_alpha*scale0 + alpha*scale1);

			return result;
		}

		/*
		 * Lerp between to Affine3f to get the correct interpolation, All affine should not contain the shear
		 * components.
		 */
		inline Eigen::Affine3f lerp(float alpha, const Eigen::Affine3f &aff0, const Eigen::Affine3f &aff1)
		{
			Eigen::Vector3f pos0; Eigen::Quaternionf rot0; Eigen::Vector3f scale0;
			Eigen::Vector3f pos1; Eigen::Quaternionf rot1; Eigen::Vector3f scale1;
			lerpDecompose(aff0, pos0, rot0, scale0);
			lerpDecompose(aff1, pos1, rot1, scale1);

			if (rot0.dot(rot1) < 0.0f)
				rot1 = Eigen::Quaternionf(-rot1.w(), -rot1.x(), -rot1.y(), -rot1.z());

			return lerpCompose(alpha, pos0, rot0, scale0, pos1, rot1, scale1);
		}

		/*
		 * Projection a vector to a normalized direction, get the tangent and normal vector values.
		 * The direction should be normalized !!!
		 */
		inline void projection(const Eigen::Vector3f & v, const Eigen::Vector3f & dir, Eigen::Vector3f & outVn, Eigen::Vector3f & outVt) {
			outVn = v.dot(dir) * dir;
			outVt = v - outVn;
		}
	}
}